/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    DesiredCoMState.h
  * @author  Mentos Seetoo
  * @brief   			
  * @note    THe following third-party libraries are used:
  *          - Eigen3
  * @warning 
  *     - At least C++11 is required.												 
  ******************************************************************************
  */
#ifndef _DESIRED_STATE_H
#define _DESIRED_STATE_H
/* Includes ------------------------------------------------------------------*/
#include <vector>
#include "Utilities/additionTypes.h"

namespace Quadruped
{

template <typename T>
struct DesiredConstrain
{
    T maxVx, minVx;
    T maxVy, minVy;
    T maxVz, minVz;
    T maxW;
    T maxRoll, minRoll;
    T maxPitch, minPitch;
    T maxYaw, minYaw;
    T maxHeight, minHeight;
    T dead_band;
};

template <typename T>
class DesiredCoMState
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    DesiredCoMState(float period)
    {
        v_d = Vec3<T>::Zero();
        max_a=0;
        w_d = Vec3<T>::Zero();
        rpy_d = Vec3<T>::Zero();
        p_d = Vec3<T>::Zero();
        for(int i = 0; i < 4; i++)
            footFrame_bais[i] = Vec3<T>::Zero();
        CoM_height = 0;
        dt = period;
    };
    T max_a;
    Vec3<T> v_in, w_in, rpy_in;
    Vec3<T> p_d,v_d, w_d, rpy_d;    //!< Desired command.

    void SetConstrains(const DesiredConstrain<T> &param);
    void SetVelocityCommand(T Vx, T Vy, T Vz,T a);
    void SetAttitudeCommand(T roll, T pitch, T yaw, T h);
    void SetHeightCommand(T height);
    void SetBeforeJumpHeightCommand(T beforeJumpHgt);
    void SetAfterJumpHeightCommand(T afterJumpHgt);
    void SetBoundJumpHeightCommand(T boundJumpHgt);
    const Vec3<T>& GetPositionCommand();
    void CalCoMCommand();
    std::vector<Vec3<T>>    footFrame_bais = std::vector<Vec3<T>>(4);   //!< vector convert H frame to foot frame
    T   CoM_height;
    T   CoM_beforeJumpHeight;// the height of CoM before jumping.
    T   CoM_afterJumpHeight;//  the height of CoM after  jumping.
    T   CoM_boundJumpHeight;
    T   dt;                     //!< Period       

private:
    /* data */
    DesiredConstrain<T>     ConstrainParam;
    std::vector<Vec3<T>>    nextFootStep = std::vector<Vec3<T>>(4);     
};
} // namespace Quadruped
#endif //DESIRED_STATE_H
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/